﻿// =====================================================================
// Physics.cs
// by Hushel Roberts
// =====================================================================

using System;
using System.Diagnostics;
using Tank;
using Microsoft.Xna.Framework;
using MathUtil;
using Geometry;

namespace Physics
{
    public enum PhysicsType
    {
        PHYS_NONE = 0,
        PHYS_DYNAMIC,
        PHYS_STATIC,
        PHYS_STATIC_FORCEAPPLIER
    }

    static public class PhysicsBase
    {
        static public bool PointInRect2D(Vector2 point, Rectangle rect)
        {
            if (point.X >= rect.X && point.X <= rect.X + rect.Width &&
                point.Y >= rect.Y && point.Y <= rect.Y + rect.Height)
                return true;
            return false;
        }

        static public bool AreCirclesColliding(Vector2 c1, float r1, Vector2 c2, float r2)
        {
            float distToCent = (c1 - c2).Length();
            float totRad = r1 + r2;
            if (distToCent <= totRad)
            {
                return true;
            }
            return false;
        }

        static public bool AreSpheresColliding(BoundingSphere sphere1, BoundingSphere sphere2)
        {
            Vector3 pos1 = sphere1.Center;
            Vector3 pos2 = sphere2.Center;

            float distToCenters = (pos1 - pos2).Length();
            float sumRadius = sphere1.Radius + sphere2.Radius;

            if (distToCenters <= sumRadius)
            {
                 return true;
            }
            return false;
        }


        // Keeps the ball inside the game world and returns false if it hits the bottom
        static public bool ContainCircleSpriteInRect(CircularSprite sprite, Rectangle rect)
        {
            Vector3 center = sprite.BoundSphere.Center;
            float radius = sprite.BoundSphere.Radius;
            float coeffOfRest = 0.8f;

            // Check for collision against top
            if (center.Y - radius <= rect.Top)
            {
                float penetrationY = (rect.Top - (center.Y - radius));
                sprite.Position = new Vector2(sprite.Position.X, sprite.Position.Y + penetrationY);
                sprite.Velocity = new Vector2(sprite.Velocity.X, sprite.Velocity.Y * -1 * coeffOfRest);
            }
            // Check collsion against bottom
            else if (center.Y + radius >= rect.Bottom)
            {
                float penetrationY = ((center.Y + radius) - rect.Bottom);
                sprite.Position = new Vector2(sprite.Position.X, sprite.Position.Y - penetrationY);
                sprite.Velocity = new Vector2(sprite.Velocity.X, sprite.Velocity.Y * -1 * coeffOfRest);
            }
            // Check for collison right side
            else if (center.X + radius >= rect.Right)
            {
                float penetrationX = ((center.X + radius) - rect.Right);
                sprite.Position = new Vector2(sprite.Position.X - penetrationX, sprite.Position.Y);
                sprite.Velocity = new Vector2(sprite.Velocity.X * -1 * coeffOfRest, sprite.Velocity.Y);
            }
            // Check for collison left side
            else if (center.X - radius <= rect.Left)
            {
                float penetrationX = (rect.Left - (center.X - radius));
                sprite.Position = new Vector2(sprite.Position.X + penetrationX, sprite.Position.Y);
                sprite.Velocity = new Vector2(sprite.Velocity.X * -1 * coeffOfRest, sprite.Velocity.Y);
            }
            return true;
        }

        static public bool ResolveCircSprites(CircularSprite sprite1, CircularSprite sprite2)
        {
            Vector3 direction = sprite1.BoundSphere.Center - sprite2.BoundSphere.Center;
            float dirMag = direction.Length();
            float radi1 = sprite1.BoundSphere.Radius;
            float radi2 = sprite2.BoundSphere.Radius;
            float overlap = (radi1 + radi2) - dirMag;

            // Test for intersection
            if (overlap > 0)
            {
                Vector2 dir = new Vector2(direction.X, direction.Y);
                dir.Normalize();

                // Resovle collision
                // Sprite 2 is static, and sprite1 force applier
                if (sprite1.PhysType == PhysicsType.PHYS_DYNAMIC || sprite2.PhysType == PhysicsType.PHYS_DYNAMIC)
                {
                    float sprt1Vel = sprite1.Velocity.Length();
                    float sprt2Vel = sprite2.Velocity.Length();

                    // Stop and push back first sprite if other is not moving
                    if (sprt1Vel > 0 && sprt2Vel == 0)
                    {
                        sprite1.Position += dir * overlap;
                    }
                    else if (sprt2Vel > 0 && sprt1Vel == 0)
                    {
                        sprite2.Position += -dir * overlap;
                    }
                    else
                    {
                        sprite1.Position += dir * overlap * 0.5f;
                        sprite2.Position += -dir * overlap * 0.5f;
                    }
                }
                else if (sprite1.PhysType == PhysicsType.PHYS_STATIC_FORCEAPPLIER)
                {
                    float finalForceMag = sprite1.ForceMag * sprite1.Elasticity * -1;
                    sprite2.ApplyAccel(new Vector2(direction.X * finalForceMag, direction.Y * finalForceMag));
                }
                // Sprite 1 is static, and sprite2 force applier
                else if (sprite2.PhysType == PhysicsType.PHYS_STATIC_FORCEAPPLIER)
                {
                    float finalForceMag = sprite2.ForceMag * sprite2.Elasticity;
                    sprite1.ApplyAccel(new Vector2(direction.X * sprite2.ForceMag, direction.Y * finalForceMag));
                }
                return true;
            }
            return false;
        }

        // This simulates the spheree as box to box collision. Only use for this tank game
        static public bool IsSphereCollidingWithRect2D(BoundingSphere sphere, BoundingBox box)
        {
            float bottomSpherePt = sphere.Center.Y + sphere.Radius;
            float topSpherePt = sphere.Center.Y - sphere.Radius;
            float leftSpherePt = sphere.Center.X - sphere.Radius;
            float rightSpherePt = sphere.Center.X + sphere.Radius;

            // Top Collision
            if (bottomSpherePt >= box.Min.Y && topSpherePt < box.Min.Y &&
                rightSpherePt >= box.Min.X && leftSpherePt <= box.Max.X)
                return true;

            // Bottom Collision
            if (topSpherePt <= box.Max.Y && bottomSpherePt > box.Max.Y &&
                rightSpherePt >= box.Min.X && leftSpherePt <= box.Max.X)
                return true;

            // Right Collision
            if (rightSpherePt >= box.Min.X && leftSpherePt < box.Min.X &&
                bottomSpherePt >= box.Min.Y && topSpherePt <= box.Max.Y)
                return true;

            // Left Collision
            if (leftSpherePt <= box.Max.X && rightSpherePt > box.Max.X &&
                topSpherePt <= box.Max.Y && bottomSpherePt >= box.Min.Y)
                return true;

            return false;
        }

        static public bool ResolveCircleToSquareSprites(CircularSprite cirSprite, SquareSprite sqrSprite)
        {
            // Test for collision
            bool intersects = IsSphereCollidingWithRect2D(cirSprite.BoundSphere, sqrSprite.BoundBox);
            if (!intersects)
                return false;

            // Find closest point on circle to square (2D collision)
            Vector2 direction = new Vector2();
            direction = sqrSprite.Position - cirSprite.Position;
            direction.Normalize();

            // Sprite 2 is static, and sprite1 dynamic
            if (cirSprite.PhysType == PhysicsType.PHYS_DYNAMIC || sqrSprite.PhysType == PhysicsType.PHYS_STATIC)
            {
                float penetrationX = -1;
                float penetrationY = -1;
                bool hitTop = false;
                bool hitLeft = false;
                // Find penetration in Y
                // Top
                if (cirSprite.BoundSphere.Center.Y + cirSprite.BoundSphere.Radius >= sqrSprite.BoundBox.Min.Y &&
                    cirSprite.BoundSphere.Center.Y - cirSprite.BoundSphere.Radius < sqrSprite.BoundBox.Min.Y)
                {
                    penetrationY = (cirSprite.BoundSphere.Center.Y + cirSprite.BoundSphere.Radius) - sqrSprite.BoundBox.Min.Y;
                    hitTop = true;
                }
                // Bottom
                else if (cirSprite.BoundSphere.Center.Y - cirSprite.BoundSphere.Radius <= sqrSprite.BoundBox.Max.Y &&
                         cirSprite.BoundSphere.Center.Y + cirSprite.BoundSphere.Radius > sqrSprite.BoundBox.Max.Y)
                {
                    penetrationY = sqrSprite.BoundBox.Max.Y - (cirSprite.BoundSphere.Center.Y - cirSprite.BoundSphere.Radius);
                }
                
                // Left
                if (cirSprite.BoundSphere.Center.X + cirSprite.BoundSphere.Radius >= sqrSprite.BoundBox.Min.X &&
                         cirSprite.BoundSphere.Center.X - cirSprite.BoundSphere.Radius < sqrSprite.BoundBox.Min.X)
                {
                    penetrationX = (cirSprite.BoundSphere.Center.X + cirSprite.BoundSphere.Radius) - sqrSprite.BoundBox.Min.X;
                    hitLeft = true;
                }
                // Right
                else if (cirSprite.BoundSphere.Center.X - cirSprite.BoundSphere.Radius <= sqrSprite.BoundBox.Max.X &&
                         cirSprite.BoundSphere.Center.X + cirSprite.BoundSphere.Radius > sqrSprite.BoundBox.Max.X)
                {
                    penetrationX = sqrSprite.BoundBox.Max.X - (cirSprite.BoundSphere.Center.X - cirSprite.BoundSphere.Radius);
                }

                Debug.Assert(penetrationY != -1 || penetrationX != -1);     // Make sure we found a penetration distance

                // Find which side of 2D square the dynamic sprite hit
                // Above square
                bool resolved = false;
                if (hitTop)
                {
                    if (penetrationX == -1 || penetrationY < penetrationX)
                    {
                        cirSprite.Position = new Vector2(cirSprite.Position.X ,cirSprite.Position.Y - penetrationY);
                        resolved = true;
                    }
                }
                // Bottom
                else if(penetrationY != -1)
                {
                    if (penetrationX == -1 || penetrationY < penetrationX)
                    {
                        cirSprite.Position = new Vector2(cirSprite.Position.X ,cirSprite.Position.Y + penetrationY);
                        resolved = true;
                    }
                }

                // Left
                if (hitLeft && !resolved)
                {
                    cirSprite.Position = new Vector2(cirSprite.Position.X - penetrationX, cirSprite.Position.Y);
                    resolved = true;
                }
                // Right
                else if (!resolved)
                {
                    cirSprite.Position = new Vector2(cirSprite.Position.X + penetrationX, cirSprite.Position.Y);
                    resolved = true;
                }
            }
            return true;
        }

        static public void ResolveBallToLine(CircularSprite sprite, Line line)
        {
            Vector2 linDirection = line.p2 - line.p1;
            Vector2 lineNormal = new Vector2(linDirection.Y, -linDirection.X);
            Vector2 ballToP1 = new Vector2((sprite.Position.X - line.p1.X), (sprite.Position.Y - line.p1.Y));
            Vector2 ballToP2 = new Vector2((sprite.Position.X - line.p2.X), (sprite.Position.Y - line.p2.Y));
            float linMag = linDirection.Length();
            float magBallToP1 = ballToP1.Length();
            float magBallToP2 = ballToP2.Length();
            float dotProd = Vector2.Dot(ballToP1, linDirection);
            float ballRad = sprite.BoundSphere.Radius;

            float magTemp = (magBallToP1 * lineNormal.Length());
            if (magTemp == 0.0f)
            {
                return;
            }

            float cosTheta = dotProd / magTemp;
            float theta = (float)Math.Acos((double)cosTheta);
            float currLen = magBallToP1;

            // Check to see if ball is outside of possible area of interseciton
            if (magBallToP1 > linMag + ballRad || magBallToP2 > linMag + ballRad)
            {
                return;
            }

            // If the angle is greater than 90 deg, use other angle
            if (theta > 90)
            {
                dotProd = Vector2.Dot(ballToP2, linDirection * -1);
                cosTheta = dotProd / (magBallToP2 * lineNormal.Length());
                currLen = magBallToP2;
            }

            //int numInterps = 3;
            //Vector2[] positions = Lerp(sprite.Position, sprite.PrevPos, numInterps);
            //int index = positions.Length - 1;
            //for (int i = index; i >= 0; --i)
            //{ 
            //    // Find distance
            //    if(positions[i]
            //    index = i;
            //}

            // Find distance from ball center to line
            float height = (float)Math.Sin((double)theta) * currLen;

            // Test for intersection
            if (height <= ballRad)
            {
                // Resolve Collision by finding theta of previous position and compare
                // Reflect
                sprite.Velocity = MathLib.ReflectVector2(sprite.Velocity, lineNormal);

                // Repostion
                lineNormal.Normalize();
                float magBallToLine = ballRad - height;
                sprite.Position = sprite.Position + (lineNormal * magBallToLine);
            }
        }

        // Returns an interpolated vector of positions of size numInterps
        static public Vector2[] Lerp(Vector2 pos, Vector2 prevPos, int numInterps)
        {
            Vector2[] vecArray = new Vector2[numInterps];
            for (int i = 0; i < numInterps; ++i)
            {
                Vector2 dir = new Vector2();
                dir = pos - prevPos;
                vecArray[i] = new Vector2();
                vecArray[i] = prevPos + (((float)(i + 1) / (float)numInterps) * dir);
            }
            return vecArray;
        }
    }

}


